import os
import sys
import time
import math
import select
import tty
import termios

from onboardDevSDK.drone_api import *

help_msg = """
GCS Controller!
---------------------------
h   : help message
q   : to quit


[   : arm
]   : disarm
t   : takeoff
l   : land
f   : frame select
w   : fly forward
s   : fly backward
a   : fly left
d   : fly right
j   : fly heigher
m   : fly lower
n   : turn left
,   : turn right

0   : test0, print drone status
1   : test1, yaw control
2   : test2, height control
3   : test3, global position control, fly a rect
4   : test4, global position control, fly a rect
5   : test5, global control, with no offboard
6   : test6, local position control 
7   : test7, user position control 
8   : test8, height control in user frame
9   : test9, accelerate control
"""
###################################################################
# get key pressed
###################################################################
settings = termios.tcgetattr(sys.stdin)


def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key
####################################################################
# 1. 通过串口连接飞控，这里连接飞控的USB口（固件烧写端口），仅仅用于地面调试，不能用于飞行测试！
# dev_addr = "serial:///dev/ttyACM0:115200"

# 2. 通过串口连接飞控，这里连接飞控的TELEM2，实际飞行时使用这个！
# dev_addr = "serial:///dev/ttyUSB0:921600"

# 3. 连接PX飞控，通过CUAV Serial转WiFi模块产生的热点连接飞控。
# dev_addr = "udp://:14550"


# 4. 连接PX4仿真器，如gazebo, jmavsim等。
# dev_addr = "udp://:14540@192.168.43.59:14580"
# dev_addr = "udp://:14541@127.0.0.1:14581"
dev_addr = "udp://:14540@127.0.0.1:14580"
# dev_addr = "udp://:14540@192.168.1.24:14580"

# 5. 其他
# dev_addr = "udp://:24540@127.0.0.1:34580"

# 设置消息转发目的地址（一般是地面站的地址）
gcs_addr = "udp://:14100@192.168.1.201:14110"
####################################################################
# parameters for PX4
params = {'sys_id': 1,
          'cmp_id': 200,
          'tgt_sys_id': 1,
          'tgt_cmp_id': 1,
          'fcu_url': dev_addr,
          #   "gcs_url":gcs_addr,
          'fcu_type': 'px4'
          }

# parameters for airsim(注意onboardDevSDK版本说明，有的版本不支持这个功能)
# params = {'drone_type':'drone',
#           'drone_id':0,
#           'origin_position':(0,0,0),
#           'fcu_url':dev_addr,
#           'fcu_type':'airsim'
#           }

drone = DroneAPI(params)
###########################################################################


def test0():
    print('INFO:test0, print drone status')
    # drone.set_user_frame((47.397742, 8.545594, 0), 0)
    for i in range(30):
        # print('--------------------------------------------------------')
        # print([a*180/3.14 for a in drone.fcu.attitude_ENU])
        print([a*180/3.14 for a in drone.fcu.attitude_NED])
        # print(drone.fcu.msg_hz)

        # print('gp: ', drone.fcu.global_position_alt_rel)
        # up = drone.get_user_position()
        # print('up: ', up)
        time.sleep(0.1)


def test1():
    print('INFO:test1, turn to yaw')
    drone.offboard_enable()
    drone.offboard_enable()
    while True:
        delta = drone.rotateto_yaw(90 * math.pi / 180)
        # delta = drone.turnto_yaw(90 * math.pi / 180)
        print(delta)
        if delta < 1 * math.pi / 180:
            break
        time.sleep(0.05)


def test2():
    print("INFO:test2, offboard control, fly to a relative height")
    # 机载计算机获取飞控控制权，千万不要写在while中，如果没有获取到（QGC显示模式不是offboard），就需要遥控器7通道强制飞控执行offboard模式
    drone.offboard_enable()
    drone.offboard_enable()
    while True:
        # 下发控制指令使无人机到达指定高度，并实时返回差值
        dis = drone.moveto_relAlt(10)
        print(dis)
        if abs(dis) < 0.2:
            break
        # 必须保证机载控制指令下发频率大于10Hz
        time.sleep(0.05)


def test3():
    print('INFO:test3, offboard control, fly a rect')
    gps = [(47.3977597, 8.5459459, 15), (47.3979504, 8.5459459, 15),
           (47.3979575, 8.5453954, 10), (47.3976255, 8.5454137, 10)]
    drone.offboard_enable()
    drone.offboard_enable()
    for gp in gps:
        while True:
            dis, alt = drone.moveto_globalPosition_relAlt_yaw(
                gp, heading_mode=1, heading=0.1)
            print("INFO: dis=", dis, alt)
            if dis < 3 and abs(alt) < 1:
                print("INFO: reached wp")
                break
            time.sleep(0.05)
        print('reached', gp)


def test4():
    print('INFO:test4, offboard control, global control')
    gps = [(47.3977597, 8.5459459, 15), (47.3979504, 8.5459459, 15),
           (47.3979575, 8.5453954, 10), (47.3976255, 8.5454137, 10)]
    drone.offboard_enable()
    drone.offboard_enable()
    for gp in gps:
        while True:
            dis = drone.goto_globalPosition_relAlt(gp)
            print(dis)
            if dis < 2:
                break
            time.sleep(0.05)


def test5():
    print('INFO:test5, global control')
    # gps = [(47.3977597, 8.5459459, 10), (47.3979504, 8.5459459, 12),
    #        (47.3979575, 8.5453954, 15), (47.3976255, 8.5454137, 10)]
    gps = [(29.377622, 104.622696, 300), (29.377722, 104.622696, 320),
           (29.377722, 104.622796, 320), (29.377622, 104.622796, 300)]
    for gp in gps:
        drone.goto_globalPosition_relAlt_yaw_async(gp)
        time.sleep(10)


def test6():
    print('INFO:test6, local position control')
    lps = [(20, 0, -10), (20, 15, -10), (-20, 15, -10), (-20, -15, -10)]

    drone.offboard_enable()
    drone.offboard_enable()
    for lp in lps:
        while True:
            dis, delta = drone.goto_localPosition_pos_yaw(lp)
            print(dis, drone.fcu.local_position)
            if dis < 2:
                break
            time.sleep(0.05)


def test7():
    print('INFO:test7, user position control')
    drone.set_user_frame((47.397742, 8.545594, 0), 0)
    ups = [(10, 0, 3), (10, 8, 3), (-10, 8, 3), (-10, -8, 3), (10, -8, 3)]
    for up in ups:
        # drone.goto_userPosition_relAlt_yaw_async(up)
        drone.goto_userPosition_relAlt_yaw_async(
            up, heading_mode=2, heading=0, speed=2)
        time.sleep(10)


def test8():
    print("INFO:test8, goto height in user frame")
    drone.goto_relAlt_async(10)
    time.sleep(5)
    drone.goto_relAlt_async(2)
    time.sleep(5)

# def test9():
#     print("INFO:test9, acceleration control in user frame")
#     drone.set_user_frame((29.377570, 104.622870, 300.0), 1.57)
#     start_time = time.time()
#     drone.offboard_enable()
#     while time.time() - start_time < 5:
#         drone.moveby_user_acc_rate(0.2, 0.0, -0.0, 0)
#         print('user_velocity=', drone.get_user_velocity())
#         time.sleep(0.1)


def test9():
    print("INFO: do orbit")
    drone.fcu.cmd_do_orbit(47.3977547, 8.5457163, 16, 10, 1.0)


def main():
    print(help_msg)
    velocity = 1
    turn_rate = 0.2

    drone.set_user_frame((29.377570, 104.622870, 300.0), 1.57)
    frame_type = 'body_v'
    while True:
        key = getKey()

        if key == 'h':
            print(help_msg)
        elif key == 'q':
            drone.exit()
            sys.exit(0)
        elif key == 'e':
            print(drone)

        elif key == '0':
            test0()
        elif key == '1':
            test1()
        elif key == '2':
            test2()
        elif key == '3':
            test3()
        elif key == '4':
            test4()
        elif key == '5':
            test5()
        elif key == '6':
            test6()
        elif key == '7':
            test7()
        elif key == '8':
            test8()
        elif key == '9':
            test9()

        elif key == '[':
            drone.arm()
        elif key == ']':
            drone.disarm()
        elif key == 't':
            print('takeoff')
            drone.takeoff()
        elif key == 'l':
            print('land')
            drone.land()

        elif key == 'f':
            frame_type = input(
                "Set fly frame type, input body_v, body_a, local_v, user_v, user_a: ")

        elif key == 'w':
            drone.offboard_enable()
            # fly in body frame
            if frame_type == 'body_v':
                drone.fly_front(velocity)
            elif frame_type == 'body_a':
                # drone.moveby_bodyNED_acc_rate(0.5, 0.0, 0.0, 0.0)
                drone.moveby_bodyNED_acc_yaw(0.5, 0.0, 0.0, 1.57)
            # fly in local frame
            elif frame_type == 'local_v':
                drone.moveby_localNED_vel_rate(velocity, 0, 0, 0)
            # drone.moveby_globalNED_vel_rate(0.0, 0, 0, 0.5)
            elif frame_type == 'user_a':
                # drone.moveby_user_acc_rate(0.5, 0.0, 0.0, 0.0)
                drone.moveby_user_acc_yaw(0.5, 0.0, 0.0, 100 * 3.14 / 180)
        elif key == 's':
            drone.offboard_enable()
            # fly in body frame
            if frame_type == 'body_v':
                drone.fly_back(velocity)
            elif frame_type == 'body_a':
                # drone.moveby_bodyNED_acc_rate(-0.5, 0.0, 0.0, 0.0)
                drone.moveby_bodyNED_acc_yaw(-0.5, 0.0, 0.0, 1.57)
            # fly in local frame
            elif frame_type == 'local_v':
                drone.moveby_localNED_vel_rate(-velocity, 0, 0, 0)
            # drone.moveby_globalNED_vel_rate(-1.0, 0, 0, 0)
            elif frame_type == 'user_a':
                # drone.moveby_user_acc_rate(-0.5, 0.0, 0.0, 0.0)
                drone.moveby_user_acc_yaw(-0.5, 0.0, 0.0, 100 * 3.14 / 180)
        elif key == 'a':
            drone.offboard_enable()
            # fly in body frame
            if frame_type == 'body_v':
                drone.fly_left(velocity)
            elif frame_type == 'body_a':
                # drone.moveby_bodyNED_acc_rate(0.0, -0.5, 0.0, 0.0)
                drone.moveby_bodyNED_acc_yaw(0.0, -0.5, 0.0, 0.0)
            # fly in local frame
            elif frame_type == 'local_v':
                drone.moveby_localNED_vel_rate(0, -velocity, 0, 0)
            elif frame_type == 'user_a':
                # drone.moveby_user_acc_rate(0.0, -0.5, 0.0, 0.0)
                drone.moveby_user_acc_yaw(0.0, -0.5, 0.0, 0.0)
        elif key == 'd':
            drone.offboard_enable()
            # fly in body frame
            if frame_type == 'body_v':
                drone.fly_right(velocity)
            elif frame_type == 'body_a':
                # drone.moveby_bodyNED_acc_rate(0.0, 0.5, 0.0, 0.0)
                drone.moveby_bodyNED_acc_yaw(0.0, 0.5, 0.0, 0.0)
            # fly in local frame
            elif frame_type == 'local_v':
                drone.moveby_localNED_vel_rate(0, velocity, 0, 0)
            elif frame_type == 'user_a':
                # drone.moveby_user_acc_rate(0.0, 0.5, 0.0, 0.0)
                drone.moveby_user_acc_yaw(0.0, 0.5, 0.0, 0.0)

        elif key == 'j':
            drone.offboard_enable()
            drone.fly_higher(velocity)
        elif key == 'm':
            drone.offboard_enable()
            drone.fly_lower(velocity)

        elif key == 'n':
            drone.offboard_enable()
            # fly in body frame
            drone.turn_left(turn_rate)
        elif key == ',':
            drone.offboard_enable()
            # fly in body frame
            drone.turn_right(turn_rate)
        elif key == 'p':
            drone.hold()


if __name__ == '__main__':
    print("INFO: run demo.py")
    main()
